
#include "Vector3.h"
#include "RMatrix.h"

using namespace Riccsson::Xna::Framework;

Vector3 g_Vector3_Zero		= Vector3(0.0f, 0.0f, 0.0f);
Vector3 g_Vector3_One		= Vector3(1.0f, 1.0f, 1.0f);
Vector3 g_Vector3_UnitX		= Vector3(1.0f, 0.0f, 0.0f);
Vector3 g_Vector3_UnitY		= Vector3(0.0f, 1.0f, 0.0f);
Vector3 g_Vector3_UnitZ		= Vector3(0.0f, 0.0f, 1.0f);
Vector3 g_Vector3_Up		= Vector3(0.0f, 1.0f, 0.0f);
Vector3 g_Vector3_Down		= Vector3(0.0f, -1.0f, 0.0f);
Vector3 g_Vector3_Right		= Vector3(1.0f, 0.0f, 0.0f);
Vector3 g_Vector3_Left		= Vector3(-1.0f, 0.0f, 0.0f);
Vector3 g_Vector3_Forward	= Vector3(0.0f, 0.0f, -1.0f);
Vector3 g_Vector3_Backward	= Vector3(0.0f, 0.0f, 1.0f);

const Vector3& Vector3::Zero	= g_Vector3_Zero;
const Vector3& Vector3::One		= g_Vector3_One;
const Vector3& Vector3::UnitX	= g_Vector3_UnitX;
const Vector3& Vector3::UnitY	= g_Vector3_UnitY;
const Vector3& Vector3::UnitZ	= g_Vector3_UnitZ;
const Vector3& Vector3::Up		= g_Vector3_Up;
const Vector3& Vector3::Down	= g_Vector3_Down;
const Vector3& Vector3::Right	= g_Vector3_Right;
const Vector3& Vector3::Left	= g_Vector3_Left;
const Vector3& Vector3::Forward	= g_Vector3_Forward;
const Vector3& Vector3::Backward= g_Vector3_Backward;




Vector3::Vector3()
{
	this->X = 0;
	this->Y = 0;
	this->Z = 0;
}

Vector3::Vector3(float x, float y, float z)
{
	this->X = x;
	this->Y = y;
	this->Z = z;
}


Vector3::Vector3(float value)
{
	this->X = value;
	this->Y = value;
	this->Z = value;
}


Vector3::Vector3(Vector2 value, float z)
{
	this->X = value.X;
	this->Y = value.Y;
	this->Z = z;
}


#pragma endregion Constructors


#pragma region Methods

Vector3 Vector3::Add(Vector3 value1, Vector3 value2)
{
	value1.X += value2.X;
	value1.Y += value2.Y;
	value1.Z += value2.Z;
	return value1;
}

void Vector3::Add(Vector3 ref value1, Vector3 ref value2, Vector3 out result)
{
	result.X = value1.X + value2.X;
	result.Y = value1.Y + value2.Y;
	result.Z = value1.Z + value2.Z;
}

Vector3 Vector3::Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2)
{
	return Vector3(
		MathHelper::Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
		MathHelper::Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
		MathHelper::Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
}

void Vector3::Barycentric(Vector3 ref value1, Vector3 ref value2, Vector3 ref value3, float amount1, float amount2, Vector3 out result)
{
	result = Vector3(
		MathHelper::Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
		MathHelper::Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
		MathHelper::Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
}

Vector3 Vector3::CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount)
{
	return Vector3(
		MathHelper::CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
		MathHelper::CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
		MathHelper::CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
}

void Vector3::CatmullRom(Vector3 ref value1, Vector3 ref value2, Vector3 ref value3, Vector3 ref value4, float amount, Vector3 out result)
{
	result = Vector3(
		MathHelper::CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
		MathHelper::CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
		MathHelper::CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
}

Vector3 Vector3::Clamp(Vector3 value1, Vector3 min, Vector3 max)
{
	return Vector3(
		MathHelper::ClampF(value1.X, min.X, max.X),
		MathHelper::ClampF(value1.Y, min.Y, max.Y),
		MathHelper::ClampF(value1.Z, min.Z, max.Z));
}

void Vector3::Clamp(Vector3 ref value1, Vector3 ref min, Vector3 ref max, Vector3 out result)
{
	result = Vector3(
		MathHelper::ClampF(value1.X, min.X, max.X),
		MathHelper::ClampF(value1.Y, min.Y, max.Y),
		MathHelper::ClampF(value1.Z, min.Z, max.Z));
}

Vector3 Vector3::Cross(Vector3 vector1, Vector3 vector2)
{
	Cross(/*ref*/ vector1, /*ref*/ vector2, /*out*/ vector1);
	return vector1;
}

void Vector3::Cross(Vector3 ref vector1, Vector3 ref vector2, Vector3 out result)
{
	result = Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z,
							-(vector1.X * vector2.Z - vector2.X * vector1.Z),
							vector1.X * vector2.Y - vector2.X * vector1.Y);
}

void Vector3::DistanceSquared(Vector3 ref value1, Vector3 ref value2, float out result)
{
	result = (value1.X - value2.X) * (value1.X - value2.X) +
				(value1.Y - value2.Y) * (value1.Y - value2.Y) +
				(value1.Z - value2.Z) * (value1.Z - value2.Z);
}

float Vector3::Distance(Vector3 vector1, Vector3 vector2)
{
	float result;
	DistanceSquared(/*ref*/ vector1, /*ref*/ vector2, /*out*/ result);
	return (float)Riccsson::System::Math::Sqrt(result);
}

void Vector3::Distance(Vector3 ref value1, Vector3 ref value2, float out result)
{
	DistanceSquared(/*ref*/ value1, /*ref*/ value2, /*out*/ result);
	result = (float)Riccsson::System::Math::Sqrt(result);
}

float Vector3::DistanceSquared(Vector3& value1, Vector3& value2)
{
	float result;
	DistanceSquared(/*ref*/ value1, /*ref*/ value2, /*out*/ result);
	return result;
}

Vector3 Vector3::Divide(Vector3 value1, Vector3 value2)
{
	value1.X /= value2.X;
	value1.Y /= value2.Y;
	value1.Z /= value2.Z;
	return value1;
}

Vector3 Vector3::Divide(Vector3 value1, float value2)
{
	float factor = 1 / value2;
	value1.X *= factor;
	value1.Y *= factor;
	value1.Z *= factor;
	return value1;
}

void Vector3::Divide(Vector3 ref value1, float divisor, Vector3 out result)
{
	float factor = 1 / divisor;
	result.X = value1.X * factor;
	result.Y = value1.Y * factor;
	result.Z = value1.Z * factor;
}

void Vector3::Divide(Vector3 ref value1, Vector3 ref value2, Vector3 out result)
{
	result.X = value1.X / value2.X;
	result.Y = value1.Y / value2.Y;
	result.Z = value1.Z / value2.Z;
}

float Vector3::Dot(Vector3 vector1, Vector3 vector2)
{
	return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
}

void Dot(Vector3 ref vector1, Vector3 ref vector2, float out result)
{
	result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
}

bool Vector3::Equals(const Vector3* other) const
{
	return ( instanceof_const( other, Vector3 ) ) ? this == (Vector3*)other : false;
}

bool Vector3::Equals(const Riccsson::System::Object* other) const
{
	//return this == other;
	throw;
}

int Vector3::GetHashCode()
{
	return (int)(this->X + this->Y + this->Z);
}

Vector3 Vector3::Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)
{
	Vector3 result = Vector3();
	Hermite(/*ref*/ value1, /*ref*/ tangent1, /*ref*/ value2, /*ref*/ tangent2, amount, /*out*/ result);
	return result;
}

void Vector3::Hermite(Vector3 ref value1, Vector3 ref tangent1, Vector3 ref value2, Vector3 ref tangent2, float amount, Vector3 out result)
{
	result.X = MathHelper::Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
	result.Y = MathHelper::Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
	result.Z = MathHelper::Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
}

float Vector3::Length()
{
	//float result;
	//Vector3::DistanceSquared(/*ref*/ this, /*ref*/ Zero, /*out*/ result);
	//return (float)Riccsson::System::Math::Sqrt(result);
	throw;
}

float Vector3::LengthSquared()
{
	//float result;
	//Vector3::DistanceSquared(/*ref*/ this, /*ref*/ Zero, /*out*/ result);
	//return result;
	throw;
}

Vector3 Vector3::Lerp(Vector3 value1, Vector3 value2, float amount)
{
	return Vector3(
		MathHelper::Lerp(value1.X, value2.X, amount),
		MathHelper::Lerp(value1.Y, value2.Y, amount),
		MathHelper::Lerp(value1.Z, value2.Z, amount));
}

void Vector3::Lerp(Vector3 ref value1, Vector3 ref value2, float amount, Vector3 out result)
{
	result = Vector3(
		MathHelper::Lerp(value1.X, value2.X, amount),
		MathHelper::Lerp(value1.Y, value2.Y, amount),
		MathHelper::Lerp(value1.Z, value2.Z, amount));
}
                
Vector3 Vector3::Max(Vector3 value1, Vector3 value2)
{
	return Vector3(
		MathHelper::Max(value1.X, value2.X),
		MathHelper::Max(value1.Y, value2.Y),
		MathHelper::Max(value1.Z, value2.Z));
}

void Vector3::Max(Vector3 ref value1, Vector3 ref value2, Vector3 out result)
{
	result = Vector3(
		MathHelper::Max(value1.X, value2.X),
		MathHelper::Max(value1.Y, value2.Y),
		MathHelper::Max(value1.Z, value2.Z));
}

Vector3 Vector3::Min(Vector3 value1, Vector3 value2)
{
	return Vector3(
		MathHelper::Min(value1.X, value2.X),
		MathHelper::Min(value1.Y, value2.Y),
		MathHelper::Min(value1.Z, value2.Z));
}

void Vector3::Min(Vector3 ref value1, Vector3 ref value2, Vector3 out result)
{
	result = Vector3(
		MathHelper::Min(value1.X, value2.X),
		MathHelper::Min(value1.Y, value2.Y),
		MathHelper::Min(value1.Z, value2.Z));
}

Vector3 Vector3::Multiply(Vector3 value1, Vector3 value2)
{
	value1.X *= value2.X;
	value1.Y *= value2.Y;
	value1.Z *= value2.Z;
	return value1;
}

Vector3 Vector3::Multiply(Vector3 value1, float scaleFactor)
{
	value1.X *= scaleFactor;
	value1.Y *= scaleFactor;
	value1.Z *= scaleFactor;
	return value1;
}

void Vector3::Multiply(Vector3 ref value1, float scaleFactor, Vector3 out result)
{
	result.X = value1.X * scaleFactor;
	result.Y = value1.Y * scaleFactor;
	result.Z = value1.Z * scaleFactor;
}

void Vector3::Multiply(Vector3 ref value1, Vector3 ref value2, Vector3 out result)
{
	result.X = value1.X * value2.X;
	result.Y = value1.Y * value2.Y;
	result.Z = value1.Z * value2.Z;
}

Vector3 Vector3::Negate(Vector3 value)
{
	value = Vector3(-value.X, -value.Y, -value.Z);
	return value;
}

void Vector3::Negate(Vector3 ref value, Vector3 out result)
{
	result = Vector3(-value.X, -value.Y, -value.Z);
}

Vector3 Vector3::Normalize(Vector3 vector)
{
	Normalize(/*ref*/ vector, /*out*/ vector);
	return vector;
}

void Vector3::Normalize(Vector3 ref value, Vector3 out result)
{
	//float factor;
	//Distance(/*ref*/ value, /*ref*/ Zero, /*out*/ factor);
	//factor = 1.0f / factor;
	//result.X = value.X * factor;
	//result.Y = value.Y * factor;
	//result.Z = value.Z * factor;
	throw;
}

void Vector3::Normalize()
{
	Normalize(/*ref*/ *this, /*out*/ *this);
}

Vector3 Vector3::Reflect(Vector3 vector, Vector3 normal)
{
	// I is the original array
	// N is the normal of the incident plane
	// R = I - (2 * N * ( DotProduct[ I,N] ))
	Vector3 reflectedVector;
	// inline the dotProduct here instead of calling method
	float dotProduct = ((vector.X * normal.X) + (vector.Y * normal.Y)) + (vector.Z * normal.Z);
	reflectedVector.X = vector.X - (2.0f * normal.X) * dotProduct;
	reflectedVector.Y = vector.Y - (2.0f * normal.Y) * dotProduct;
	reflectedVector.Z = vector.Z - (2.0f * normal.Z) * dotProduct;

	return reflectedVector;
}

void Vector3::Reflect(Vector3 ref vector, Vector3 ref normal, Vector3 out result)
{
	// I is the original array
	// N is the normal of the incident plane
	// R = I - (2 * N * ( DotProduct[ I,N] ))

	// inline the dotProduct here instead of calling method
	float dotProduct = ((vector.X * normal.X) + (vector.Y * normal.Y)) + (vector.Z * normal.Z);
	result.X = vector.X - (2.0f * normal.X) * dotProduct;
	result.Y = vector.Y - (2.0f * normal.Y) * dotProduct;
	result.Z = vector.Z - (2.0f * normal.Z) * dotProduct;

}
		
Vector3 Vector3::SmoothStep(Vector3 value1, Vector3 value2, float amount)
{
	return Vector3(
		MathHelper::SmoothStep(value1.X, value2.X, amount),
		MathHelper::SmoothStep(value1.Y, value2.Y, amount),
		MathHelper::SmoothStep(value1.Z, value2.Z, amount));
}

void Vector3::SmoothStep(Vector3 ref value1, Vector3 ref value2, float amount, Vector3 out result)
{
	result = Vector3(
		MathHelper::SmoothStep(value1.X, value2.X, amount),
		MathHelper::SmoothStep(value1.Y, value2.Y, amount),
		MathHelper::SmoothStep(value1.Z, value2.Z, amount));
}

Vector3 Vector3::Subtract(Vector3 value1, Vector3 value2)
{
	value1.X -= value2.X;
	value1.Y -= value2.Y;
	value1.Z -= value2.Z;
	return value1;
}

void Vector3::Subtract(Vector3 ref value1, Vector3 ref value2, Vector3 out result)
{
	result.X = value1.X - value2.X;
	result.Y = value1.Y - value2.Y;
	result.Z = value1.Z - value2.Z;
}

GC_PTR<Riccsson::System::string> Vector3::ToString()
{
	//StringBuilder sb = new StringBuilder(32);
	//sb.Append("{X:");
	//sb.Append(this->X);
	//sb.Append(" Y:");
	//sb.Append(this->Y);
	//sb.Append(" Z:");
	//sb.Append(this->Z);
	//sb.Append("}");
	//return sb.ToString();
	throw;
}

Vector3 Vector3::Transform(Vector3 position, Matrix matrix)
{
	Transform(/*ref*/ position, /*ref*/ matrix, /*out*/ position);
	return position;
}

void Vector3::Transform(Vector3 ref position, Matrix ref matrix, Vector3 out result)
{
	result = Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,
							(position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,
							(position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43);
}

void Vector3::Transform(Riccsson::System::Array<Vector3>* sourceArray, Matrix ref matrix, Riccsson::System::Array<Vector3>* destinationArray)
{
	//Debug.Assert(destinationArray.Length >= sourceArray.Length, "The destination array is smaller than the source array.");

	// TODO: Are there options on some platforms to implement a vectorized version of this?

	for (var i = 0; i < sourceArray->Length; i++)
	{
		var position = sourceArray->at(i);                
		destinationArray->at(i) =
			Vector3(
				(position.X*matrix.M11) + (position.Y*matrix.M21) + (position.Z*matrix.M31) + matrix.M41,
				(position.X*matrix.M12) + (position.Y*matrix.M22) + (position.Z*matrix.M32) + matrix.M42,
				(position.X*matrix.M13) + (position.Y*matrix.M23) + (position.Z*matrix.M33) + matrix.M43);
	}
}

/// <summary>
/// Transforms a vector by a quaternion rotation.
/// </summary>
/// <param name="vec">The vector to transform.</param>
/// <param name="quat">The quaternion to rotate the vector by.</param>
/// <returns>The result of the operation.</returns>
Vector3 Vector3::Transform(Vector3 vec, Quaternion quat)
{
	Vector3 result;
	Transform(/*ref*/ vec, /*ref*/ quat, /*out*/ result);
	return result;
}

/// <summary>
/// Transforms a vector by a quaternion rotation.
/// </summary>
/// <param name="vec">The vector to transform.</param>
/// <param name="quat">The quaternion to rotate the vector by.</param>
/// <param name="result">The result of the operation.</param>
//        void Transform(Vector3 ref vec, ref Quaternion quat, Vector3 out result)
//        {
//		// Taken from the OpentTK implementation of Vector3
//            // Since vec.W == 0, we can optimize quat * vec * quat^-1 as follows:
//            // vec + 2.0 * cross(quat.xyz, cross(quat.xyz, vec) + quat.w * vec)
//            Vector3 xyz = quat.Xyz, temp, temp2;
//            Vector3.Cross(ref xyz, ref vec, out temp);
//            Vector3.Multiply(ref vec, quat.W, out temp2);
//            Vector3.Add(ref temp, ref temp2, out temp);
//            Vector3.Cross(ref xyz, ref temp, out temp);
//            Vector3.Multiply(ref temp, 2, out temp);
//            Vector3.Add(ref vec, ref temp, out result);
//        }

/// <summary>
/// Transforms a vector by a quaternion rotation.
/// </summary>
/// <param name="vec">The vector to transform.</param>
/// <param name="quat">The quaternion to rotate the vector by.</param>
/// <param name="result">The result of the operation.</param>
void Vector3::Transform(Vector3 ref value, Quaternion ref rotation, Vector3 out result)
{
	float x = 2 * (rotation.Y * value.Z - rotation.Z * value.Y);
	float y = 2 * (rotation.Z * value.X - rotation.X * value.Z);
	float z = 2 * (rotation.X * value.Y - rotation.Y * value.X);

	result.X = value.X + x * rotation.W + (rotation.Y * z - rotation.Z * y);
	result.Y = value.Y + y * rotation.W + (rotation.Z * x - rotation.X * z);
	result.Z = value.Z + z * rotation.W + (rotation.X * y - rotation.Y * x);
}

Vector3 Vector3::TransformNormal(Vector3 normal, Matrix matrix)
{
	TransformNormal(/*ref*/ normal, /*ref*/ matrix, /*out*/ normal);
	return normal;
}

void Vector3::TransformNormal(Vector3 ref normal, Matrix ref matrix, Vector3 out result)
{
	result = Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31),
							(normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32),
							(normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33));
}

#pragma endregion methods

						
Vector3& Vector3::operator= (Vector3 value)
{
	this->X = value.X;
	this->Y = value.Y;
	this->Z = value.Z;
	return *this;
}

Vector3 Vector3::operator -(const Vector3& value)
{
	return Vector3(this->X-value.X, this->Y-value.Y,this->Z-value.Z);
}



bool Vector3::operator ==(const Vector3& value2)
{
	return this->X == value2.X
		&& this->Y == value2.Y
		&& this->Z == value2.Z;
}

bool Vector3::operator !=(Vector3 value2)
{
	return !(this->X == value2.X
		  && this->Y == value2.Y
		  && this->Z == value2.Z);
}

Vector3 Vector3::operator +(Vector3 value2)
{
	Vector3 output;
	output.X = this->X + value2.X;
	output.Y = this->Y + value2.Y;
	output.Z = this->Z + value2.Z;
	return output;
}

Vector3 Vector3::operator +=(Vector3 value2)
{
	Vector3 output;
	output.X = this->X + value2.X;
	output.Y = this->Y + value2.Y;
	output.Z = this->Z + value2.Z;
	return output;
}

//Vector3 operator -(Vector3 value)
//{
//	value = Vector3(-value.X, -value.Y, -value.Z);
//	return value;
//}

//Vector3 Vector3::operator -(Vector3 value1, Vector3 value2)
//{
//	value1.X -= value2.X;
//	value1.Y -= value2.Y;
//	value1.Z -= value2.Z;
//	return value1;
//}

//Vector3 Vector3::operator *(Vector3 value2)
//{
//	Vector3 output;
//	output.X = this->X * value2.X;
//	output.Y = this->Y * value2.Y;
//	output.Z = this->Z * value2.Z;
//	return output;
//}
//
//Vector3 Vector3::operator *(float scaleFactor)
//{
//	Vector3 output;
//	output.X = this->X * scaleFactor;
//	output.Y = this->Y * scaleFactor;
//	output.Z = this->Z * scaleFactor;
//	return output;
//}
//
//Vector3 Vector3::operator /(Vector3 value2)
//{
//	Vector3 output;
//	output.X = this->X / value2.X;
//	output.Y = this->Y / value2.Y;
//	output.Z = this->Z / value2.Z;
//	return output;
//}
//
Vector3 Vector3::operator /=(Vector3 value2)
{
	Vector3 output;
	output.X = this->X / value2.X;
	output.Y = this->Y / value2.Y;
	output.Z = this->Z / value2.Z;
	return output;
}

//Vector3 Vector3::operator /(float divider)
//{
//	Vector3 output;
//	output.X = this->X / divider;
//	output.Y = this->Y / divider;
//	output.Z = this->Z / divider;
//	return output;
//}
//
//// Globals.
//bool operator ==(const Vector3& value1, const Vector3& value2)
//{
//	return value1.X == value2.X
//		&& value1.Y == value2.Y
//		&& value1.Z == value2.Z;
//}
//Vector3 operator *(float scaleFactor, Vector3 value)
//{
//	value.X *= scaleFactor;
//	value.Y *= scaleFactor;
//	value.Z *= scaleFactor;
//	return value;
//}
